package ch.areion.areionapp.sensor;

import java.util.ArrayList;
import java.util.Collections;


import ch.areion.areionapp.communication.Monitor;
import ch.areion.common.message.Module;
import ch.areion.common.message.Type;
import ch.areion.common.message.Version;
import ioio.lib.api.DigitalOutput;
import ioio.lib.api.IOIO;
import ioio.lib.api.PulseInput;
import ioio.lib.api.PulseInput.PulseMode;
import ioio.lib.api.exception.ConnectionLostException;

public class USSensor implements IUSSensor {
	/* Settings */
	private static final int DEFAULTMSRCNT = 5;
	private static final int MSRSLEEP = 10;

	/* Pins */
	private PulseInput ussF_echo;
	private DigitalOutput ussF_trigger;
	private PulseInput ussB_echo;
	private DigitalOutput ussB_trigger;
	
	/* Logic */
	private static final boolean ON = true;
	private static final boolean OFF = !ON;
	
	/* Pin definition */
	private static final int F_OUT = 20;
	private static final int B_OUT = 21;
	private static final int F_IN = 13;
	private static final int B_IN = 14;
	
	/* IOIO instance */
	private IOIO ioio;

	public USSensor(IOIO ioio_) {
		ioio = ioio_;
		setup();
	}

	/**
	 * initialise ultrasonic sensor.
	 */
	private void setup() {
		try {
			ussF_echo = ioio.openPulseInput(F_IN, PulseMode.POSITIVE);
			ussF_trigger = ioio.openDigitalOutput(F_OUT,DigitalOutput.Spec.Mode.OPEN_DRAIN,ON);

			ussB_echo = ioio.openPulseInput(B_IN, PulseMode.POSITIVE);
			ussB_trigger = ioio.openDigitalOutput(B_OUT,DigitalOutput.Spec.Mode.OPEN_DRAIN,ON);
		} catch (ConnectionLostException e) {
			Monitor.getInstance().add(Version.V01, Type.ERROR, Module.SENSOR,
					"IOIO connection lost.");
		}
	}
	
	@Override
	public void disconnect() throws ConnectionLostException, InterruptedException
	{
		ussF_trigger.close();
		ussB_trigger.close();
		ussF_echo.close();
		ussB_echo.close();
		
	}

	@Override
	public int getDistanceUSSF() {
		return getDistanceUSSF(DEFAULTMSRCNT);
	}
	
	@Override
	public int getDistanceUSSF(int msrCnt) {
		int distance = getDistanceUSS(msrCnt, ussF_echo, ussF_trigger);
		
		Monitor.getInstance().add(Version.V01, Type.INFO, Module.SENSOR,
				"USS - Front", distance + "[mm]");
		return distance;
		
	}

	@Override
	public int getDistanceUSSB() {
		return getDistanceUSSB(DEFAULTMSRCNT);
	}
	@Override
	public int getDistanceUSSB(int msrCnt) {
		int distance = getDistanceUSS(msrCnt, ussB_echo, ussB_trigger);
		
		Monitor.getInstance().add(Version.V01, Type.INFO, Module.SENSOR,
				"USS - Rear", distance + "[mm]");
		return distance;
	}

	/**
	 * Take msrCnt of measures and return avg value.
	 * @param msrCnt Number of measurements
	 * @return The measurement or -1 if failed.
	 */
	public int getDistanceUSS(int msrCnt, PulseInput in, DigitalOutput out) {
		int distance = -1;
		ArrayList<Integer> measures = new ArrayList<Integer>();

		// take #msrcnt numbers of measurements
		try {
			for (int m = 0; m < msrCnt; m++) {
				measures.add(takeMeasurement(in, out));

				// wait for next measurement
				try {
					Thread.sleep(MSRSLEEP);
				} catch (InterruptedException e) {
					Monitor.getInstance().add(Version.V01, Type.ERROR,
							Module.SENSOR, "Unable to sleep...");
				}
			}
		} catch (ConnectionLostException e1) {
			Monitor.getInstance().add(Version.V01, Type.ERROR,
					Module.SENSOR, "Unable to take measurement on uss...");
			return -1;
		}

		// sort measurements
		Collections.sort(measures);

		// disallow removing of max/min if less than 3 values measured
		int first = 1;
		int last = msrCnt - 1;
		int eliminates = 2;
		if(msrCnt < 3) {
			first = 0;
			last = msrCnt;
			eliminates = 0;
		}
		
		// calculate distance
		distance = 0;
		for (int m = first; m < last; m++) {
			distance += measures.get(m);
		}
		distance = (int) (((float) distance) / ((float) msrCnt - eliminates));

		return distance;

	}
	
	public int takeMeasurement(PulseInput in, DigitalOutput out) throws ConnectionLostException {
		int distance = -1;

		try {
			// make sure that output is resetted
			out.write(OFF);
			Thread.sleep(5);

			// send pulse
			out.write(ON);
			Thread.sleep(1);
			out.write(OFF);

			// check time for return of signal
			float ms = (in.getDuration() * 1000);
			distance = (int) (ms / 0.0058);
		} catch (InterruptedException e) {
			Monitor.getInstance().add(Version.V01, Type.WARNING, Module.SENSOR,
					"Unable to sleep while measuring uss");
		}
		
		return distance;
	}
}
